/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.attitudes;

import ore.Attitude;
import ore.AttitudeLaw;
import ore.Frame;
import ore.Line;
import ore.PositionVelocity;
import ore.Transform;
import ore.bodies.BodyShape;
import ore.bodies.GeodeticPoint;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

/**
 * The attitude pointing law is defined by an attitude law and
 * the satellite axis vector chosen for pointing.
 * 
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class LofOffsetPointing
    extends GroundPointing
{


    /**
     * Rotation from local orbital frame.
     */
    private final AttitudeLaw attitudeLaw;
    /**
     * Body shape.
     */
    private final BodyShape shape;
    /**
     * Chosen satellite axis for pointing, given in satellite frame.
     */
    private final Vector3D satPointingVector;


    /** 
     * @param shape Body shape
     * @param attLaw Attitude law
     * @param satPointingVector Satellite vector defining the pointing direction
     */
    public LofOffsetPointing(BodyShape shape, AttitudeLaw attLaw,
                             Vector3D satPointingVector)
    {
        super(shape.getBodyFrame());
        this.shape = shape;
        this.attitudeLaw = attLaw;
        this.satPointingVector = satPointingVector;
    }
    /** 
     * Compute the system state at given date in given frame.
     *
     * User should check that position/velocity and frame is
     * consistent with given frame.
     * 
     * @param date date when system state shall be computed
     * @param pv satellite position/velocity in given frame
     * @param frame the frame in which pv is defined
     * 
     * @return satellite attitude state at date
     * 
     * @throws OrekitException if some specific error occurs
     */
    public Attitude getState(AbsoluteDate date,
                             PositionVelocity pv, Frame frame)
        throws OrekitException
    {
        return this.attitudeLaw.getState(date, pv, frame);
    }
    /**
     * Get target expressed in body frame at given date.
     * 
     * User should check that position/velocity and frame is
     * consistent with given frame.
     * 
     * @param date computation date.
     * @param pv satellite position-velocity vector at given date in given frame.
     * @param frame Frame in which satellite position-velocity is given.
     * 
     * @return Target position/velocity in body frame
     * 
     * @throws OrekitException If some specific error occurs
     */
    protected PositionVelocity getTargetInBodyFrame(AbsoluteDate date,
                                                    PositionVelocity pv, Frame frame)
        throws OrekitException
    {
        /*
         * Compute satellite state at given date in given frame
         */
        Rotation satRot = this.getState(date, pv, frame).getRotation();
        /*
         * Compute satellite pointing axis and position/velocity in body frame
         */
        Transform t = frame.getTransformTo(shape.getBodyFrame(), date);

        Vector3D pointingBodyFrame =
            t.transformVector(satRot.applyInverseTo(satPointingVector));

        Vector3D pBodyFrame = t.transformPosition(pv.getPosition());
        /*
         * Line from satellite following pointing direction
         */
        Line pointingLine = new Line(pBodyFrame, pointingBodyFrame);
        /*
         * Intersection with body shape
         */
        GeodeticPoint gpIntersection =
            shape.getIntersectionPoint(pointingLine, pBodyFrame, shape.getBodyFrame(), date);

        Vector3D vIntersection =
            (gpIntersection == null) ? null : shape.transform(gpIntersection);

        /*
         * Check there is an intersection and it is not in the reverse pointing direction
         */
        if ((vIntersection == null) ||
            (Vector3D.dotProduct(vIntersection.subtract(pBodyFrame), pointingBodyFrame) < 0))
        {
            throw new OrekitException("attitude pointing law misses ground");
        }
        else
            return new PositionVelocity(vIntersection, Vector3D.ZERO);
    }
}
